#include "daq.h"
#include "util/util.h"
#include <stdio.h>

using namespace DAQ_namespace;

extern DAQALLOC DAQ;

extern EVENT_HANDLE eh;
extern DWORD eventMsk;
extern DWORD eventWord;

extern pthread_t primary;  //this is the primary thread.  The program state machine exists here
extern pthread_t receive;  //this thread handles and buffers incoming Rx data

//GENERAL MSGS

void DAQALLOC::startConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->state = DAQ_RUNNING;
    }
    this->general.startPending = false;
}
void DAQALLOC::stopConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->state = DAQ_STOPPED_STATE;
    }
    this->general.stopPending = false;
}

void DAQALLOC::readyConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->state = DAQ_INIT_STATE;
    }
}


//ANALOG IN

void DAQALLOC::analogInConfig(uint8_t errorCode)
{
    if      (errorCode & ADC_CH0_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[0].enable = false;
        }
        else
        {
            this->analogIn[0].enable = true;
        }

        this->analogIn[0].settingPending = false;

    }
    else if (errorCode & ADC_CH1_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[1].enable = false;
        }
        else
        {
            this->analogIn[1].enable = true;
        }

        this->analogIn[1].settingPending = false;

    }
    else if (errorCode & ADC_CH2_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[2].enable = false;
        }
        else
        {
            this->analogIn[2].enable = true;
        }

        this->analogIn[2].settingPending = false;

    }
    else if (errorCode & ADC_CH3_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[3].enable = false;
        }
        else
        {
            this->analogIn[3].enable = true;
        }

        this->analogIn[3].settingPending = false;

    }
}

//ANALOG OUT

void DAQALLOC::analogOutConfig(uint8_t errorCode)
{
    if(errorCode & DAC_CH0_bp)
    {

        //if there is a way to turn it off add here

        this->analogOut[0].enable = true;
        this->analogOut[0].settingPending = false;
    }
    else if(errorCode & DAC_CH1_bp)
    {

        //if there is a way to turn it off add here

        this->analogOut[1].enable = true;
        this->analogOut[1].settingPending = false;
    }
}

//DIGITAL IN

void DAQALLOC::digitalInConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->digital.inEnable = true;
        this->digital.inSettingPending = false;
    }
}

//DIGITAL OUT

void DAQALLOC::digitalOutConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->digital.outEnable = true;
        this->digital.outSettingPending = false;
    }
}

//ENCODER

//This function handles enc config message acks
void DAQALLOC::encoderConfig(uint8_t errorCode)
{
    if      (errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->encoder[0].enable = true;
            this->encoder[0].settingPending = false;

            encDioConfig(0);
        }
        else
        {
            this->encoder[1].enable = true;
            this->encoder[1].settingPending = false;

            encDioConfig(1);
        }

    }
    else if(errorCode & 0x1)
    {
        this->encoder[0].settingPending = false;

    }
    else if(errorCode & 0x2)
    {
        this->encoder[1].settingPending = false;
    }
}

//This function backward annotates encoder i/o allocations based on the PLOC and index settings
void DAQALLOC::encDioConfig(uint8_t encNum)
{
    switch(encoder[encNum].PLOC)
    {
    case PLOC_D1:

        this->digital.dioAlloc[1] = FEATURE_UTILIZED;
        this->digital.dir[1] = DIGITAL_INPUT;
        this->digital.dioAlloc[2] = FEATURE_UTILIZED;
        this->digital.dir[2] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[3] = FEATURE_UTILIZED;
            this->digital.dir[3] = DIGITAL_INPUT;
        }

    break;
    case PLOC_D2:
        this->digital.dioAlloc[2] = FEATURE_UTILIZED;
        this->digital.dir[2] = DIGITAL_INPUT;
        this->digital.dioAlloc[3] = FEATURE_UTILIZED;
        this->digital.dir[3] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[4] = FEATURE_UTILIZED;
            this->digital.dir[4] = DIGITAL_INPUT;
        }

    break;
    case PLOC_D3:

        this->digital.dioAlloc[3] = FEATURE_UTILIZED;
        this->digital.dir[3] = DIGITAL_INPUT;
        this->digital.dioAlloc[4] = FEATURE_UTILIZED;
        this->digital.dir[4] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[5] = FEATURE_UTILIZED;
            this->digital.dir[5] = DIGITAL_INPUT;
        }

    break;
    case PLOC_D4:

        this->digital.dioAlloc[4] = FEATURE_UTILIZED;
        this->digital.dir[4] = DIGITAL_INPUT;
        this->digital.dioAlloc[5] = FEATURE_UTILIZED;
        this->digital.dir[5] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[6] = FEATURE_UTILIZED;
            this->digital.dir[6] = DIGITAL_INPUT;
        }

    break;
    case PLOC_D5:

        this->digital.dioAlloc[5] = FEATURE_UTILIZED;
        this->digital.dir[5] = DIGITAL_INPUT;
        this->digital.dioAlloc[6] = FEATURE_UTILIZED;
        this->digital.dir[6] = DIGITAL_INPUT;

    break;
    case PLOC_D10:

        this->digital.dioAlloc[10] = FEATURE_UTILIZED;
        this->digital.dir[10] = DIGITAL_INPUT;
        this->digital.dioAlloc[9] = FEATURE_UTILIZED;
        this->digital.dir[9] = DIGITAL_INPUT;

    break;
    case PLOC_D11:

        this->digital.dioAlloc[11] = FEATURE_UTILIZED;
        this->digital.dir[11] = DIGITAL_INPUT;
        this->digital.dioAlloc[10] = FEATURE_UTILIZED;
        this->digital.dir[10] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[9] = FEATURE_UTILIZED;
            this->digital.dir[9] = DIGITAL_INPUT;
        }

    break;
    case PLOC_D12:

        this->digital.dioAlloc[12] = FEATURE_UTILIZED;
        this->digital.dir[12] = DIGITAL_INPUT;
        this->digital.dioAlloc[11] = FEATURE_UTILIZED;
        this->digital.dir[11] = DIGITAL_INPUT;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[10] = FEATURE_UTILIZED;
            this->digital.dir[10] = DIGITAL_INPUT;
        }

    break;
    }
}

//MOTOR

void DAQALLOC::motorConfig(uint8_t errorCode)
{
    if(errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->motor[0].settingPending = false;
            this->motor[0].enable = true;

            this->digital.dioAlloc[1] = FEATURE_UTILIZED;
            this->digital.dir[1] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x2)
        {
            this->motor[1].settingPending = false;
            this->motor[1].enable = true;

            this->digital.dioAlloc[2] = FEATURE_UTILIZED;
            this->digital.dir[2] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x4)
        {
            this->motor[2].settingPending = false;
            this->motor[2].enable = true;

            this->digital.dioAlloc[3] = FEATURE_UTILIZED;
            this->digital.dir[3] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x8)
        {
            this->motor[3].settingPending = false;
            this->motor[3].enable = true;

            this->digital.dioAlloc[4] = FEATURE_UTILIZED;
            this->digital.dir[4] = DIGITAL_OUTPUT;
        }
    }
    else
    {
        if(errorCode & 0x1) this->motor[0].settingPending = false;
        if(errorCode & 0x2) this->motor[1].settingPending = false;
        if(errorCode & 0x4) this->motor[2].settingPending = false;
        if(errorCode & 0x8) this->motor[3].settingPending = false;
    }
}

//SERVO

void DAQALLOC::servoConfig(uint8_t errorCode)
{
    if(errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->servo[0].settingPending = false;
            this->servo[0].enable = true;

            this->digital.dioAlloc[12] = FEATURE_UTILIZED;
            this->digital.dir[12] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x2)
        {
            this->servo[1].settingPending = false;
            this->servo[1].enable = true;

            this->digital.dioAlloc[11] = FEATURE_UTILIZED;
            this->digital.dir[11] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x4)
        {
            this->servo[2].settingPending = false;
            this->servo[2].enable = true;

            this->digital.dioAlloc[10] = FEATURE_UTILIZED;
            this->digital.dir[10] = DIGITAL_OUTPUT;
        }
        if(errorCode & 0x8)
        {
            this->servo[3].settingPending = false;
            this->servo[3].enable = true;

            this->digital.dioAlloc[9] = FEATURE_UTILIZED;
            this->digital.dir[9] = DIGITAL_OUTPUT;
        }
    }
    else
    {
        if(errorCode & 0x1) this->servo[0].settingPending = false;
        if(errorCode & 0x2) this->servo[1].settingPending = false;
        if(errorCode & 0x4) this->servo[2].settingPending = false;
        if(errorCode & 0x8) this->servo[3].settingPending = false;
    }
}

//TIMER

void DAQALLOC::tmrConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->timer.enable = true;
    }
    this->timer.settingPending = false;
}


//SUPER PACKET HANDLERS


void DAQALLOC::inputSuperPacketHandler(uint8_t packet[])
{
    WORD_t val;
    LONG_WORD_t val32;

    uint8_t pos = 2;
    uint8_t inc;

    //ANALOG
    for(inc=0;inc<4;inc++)
    {
        if(this->analogIn[inc].enable)
        {
            val.bytes.byteH = packet[pos++];
            val.bytes.byteL = packet[pos++];

            // populate val somewhere
            if(inc == 3)    printf("% 2.3f\r\n",10*((int16_t) val.word)/8192.0);
            else            printf("% 2.3f\t",10*((int16_t) val.word)/8192.0);
        }
    }
    for(inc=0;inc<2;inc++)
    {
        if(this->encoder[inc].enable)
        {
            if(this->encoder[inc].absolute)
            {
                val.bytes.byteH = packet[pos++];
                val.bytes.byteL = packet[pos++];
            }
            else
            {
                val32.bytes.byteHH = packet[pos++];
                val32.bytes.byteMH = packet[pos++];
                val32.bytes.byteML = packet[pos++];
                val32.bytes.byteLL = packet[pos++];
            }
        }
    }

}
